#include "Quadric.h"
#include <iostream>
using namespace std;

Quadric::Quadric(const Point &_p, const double &_A, const double &_B, const double &_C, const double &_D, const double &_E, const double &_F, const double &_G, const double &_H, const double &_I, const double &_J)
{
	location = _p;
	A=_A; B=_B; C=_C; D=_D; E=_E; F=_F; G=_G; H=_H; I=_I; J=_J;
}
bool Quadric::intersection(Ray& ray, Intersection& i) const
{
	Point O = ray.getOrigin();
	Vector vecD = ray.getDirection();
	Point intersection;
	Vector normal;
	double Aq, Bq, Cq, t0, t1, t, delta;

	Aq =	A*vecD.get_x()*vecD.get_x() +
			B*vecD.get_y()*vecD.get_y() +
			C*vecD.get_z()*vecD.get_z() +
			D*vecD.get_x()*vecD.get_y() +
			E*vecD.get_x()*vecD.get_z() +
			F*vecD.get_y()*vecD.get_z();
	
	Bq =	2 * (A*O.get_x()*vecD.get_x() + B*O.get_y()*vecD.get_y() + C*O.get_z()*vecD.get_z()+
			D * (O.get_x()*vecD.get_y() + O.get_y()*vecD.get_x()) +
			E * (O.get_x()*vecD.get_z() + O.get_z()*vecD.get_x()) +
			F * (O.get_y()*vecD.get_z() + O.get_z()*vecD.get_y()) )+
			G * vecD.get_x() +
			H * vecD.get_y() +
			I * vecD.get_z();
	
	Cq =	A * O.get_x()*O.get_x() +
			B * O.get_y()*O.get_y() +
			C * O.get_z()*O.get_z() +
			D * O.get_x()*O.get_y() +
			E * O.get_x()*O.get_z() +
			F * O.get_y()*O.get_z() +
			G * O.get_x() +
			H * O.get_y() +
			I * O.get_z() +
			J;
	//cout<<Bq * Bq - 4 * Aq * Cq<<endl;
	if(Aq==0){ t =  -Cq / Bq; }
	else
	{
		if( (Bq * Bq - 4 * Aq * Cq) < 0) return false;
		else 
		{
			t0 =(-Bq - sqrt(Bq*Bq - 4*Aq*Cq))/ (2*Aq);
			t1 =(-Bq + sqrt(Bq*Bq - 4*Aq*Cq))/ (2*Aq);
	
			if (!(t0<0.001 && t1<0.001))
			{
				if( t0<=0.001)
				{
					t=t1;
				}
				else
				{
					t=t0;
				}
				//t=t0<t1?t0:t1;
				//cout<<t<<" " <<t0<<" "<<t1<<endl;
				//cout<<O.get_x()+vecD.get_x()*t0<<" "<<O.get_y()+vecD.get_y()*t0<<" "<<O.get_z()+vecD.get_z()*t0<<endl;
				//cout<<O.get_x()+vecD.get_x()*t1<<" "<<O.get_y()+vecD.get_y()*t1<<" "<<O.get_z()+vecD.get_z()*t1<<endl;
				//cout<<"---------"<<endl;			
				double x0 = (O.get_x() - location.get_x());
				double y0 = (O.get_y() - location.get_y());
				double z0 = (O.get_z() - location.get_z());

				double xd = vecD.get_x();
				double yd = vecD.get_y();
				double zd = vecD.get_z();

				intersection = Point(O.get_x()+vecD.get_x()*t, O.get_y()+vecD.get_y()*t, O.get_z()+vecD.get_z()*t);
				
				//cout << intersection.get_x() << " " << intersection.get_y() << " " << intersection.get_z()<<endl;
				normal =Vector((2 * A * ( (O.get_x() - location.get_x()) + vecD.get_x() * t) + 
								2 * F * ( (O.get_y() - location.get_y()) + vecD.get_y() * t) + 
								2 * E * ( (O.get_z() - location.get_z()) + vecD.get_z() * t) + G - location.get_x()),
							   (2 * B * ( (O.get_y() - location.get_y()) + vecD.get_y() * t) +
							    2 * F * ( (O.get_x() - location.get_x()) + vecD.get_x() * t) +
							    2 * D * ( (O.get_z() - location.get_z()) + vecD.get_z() * t) + H - location.get_y()),
							   (2 * C * ( (O.get_z() - location.get_z()) + vecD.get_z() * t) +
								2 * E * ( (O.get_x() - location.get_x()) + vecD.get_x() * t) +
								2 * D * ( (O.get_y() - location.get_y()) + vecD.get_y() * t) + I - location.get_z()));
			
				i.setC(color);
				//cout<<color.getR()<<endl;
				normal.normalize();
				//normal.display();
				i.setNormal(normal);
				//i.setNormal(Vector(0,0,-1));
				i.setP(intersection);
				//intersection.display();
				i.setDistance(t);
				//cout<<t<<endl;
				//cout<<endl;
				return(true);
			}
		}
	}
	return(false);
}

Vector Quadric::getNormal (const Point& p) const
{
	Vector res = Vector();
	res.normalize();
	return res;
}
